#include "AiboHeadControl.h"


AiboHeadControl::AiboHeadControl( HeadInstructionHandler* headHandler )
{
	this->setHeadHandler( headHandler );
	this->centerHead();
}

AiboHeadControl::~AiboHeadControl(void)
{
}

void AiboHeadControl::setTilt( double tilt )
{
	if( this->getHeadHandler() != NULL && this->getTilt() != tilt ) {
		this->m_tilt = this->convertToWithinOne( tilt );
		this->getHeadHandler()->addInstruction( HeadTiltInstruction( this->getTilt() ) );
		qDebug() << "Head Control: Tilt Set - " << this->getTilt() << endl;
	}
}

void AiboHeadControl::setPan( double pan )
{
	if( this->getHeadHandler() != NULL && this->getPan() != pan ) {
		this->m_pan = this->convertToWithinOne( pan );
		this->getHeadHandler()->addInstruction( HeadPanInstruction( this->getPan() ) );
		qDebug() << "Head Control: Pan Set - " << this->getPan() << endl;
	}
}

void AiboHeadControl::setRoll( double roll )
{
	if( this->getHeadHandler() != NULL && this->getRoll() != roll ) {
		this->m_roll = this->convertToWithinOne( roll );
		this->getHeadHandler()->addInstruction( HeadRollInstruction( this->getRoll() ) );
		qDebug() << "Head Control: Roll Set - " << this->getRoll() << endl;
	}
}

void AiboHeadControl::rollUp() {
	if( this->getRoll() + 0.05f <= 1.0f ) {
		this->setRoll( this->getRoll() + 0.1f );
		qDebug() << "Head Control: Rolling Up - " << this->getRoll() << endl;
	}
}

void AiboHeadControl::rollDown() {
	if( this->getRoll() - 0.01f >= -1.0f ) {
		this->setRoll( this->getRoll() - 0.1f );
		qDebug() << "Head Control: Rolling Down - " << this->getRoll() << endl;
	}
}

void AiboHeadControl::panLeft() {
	if( this->getPan() + 0.01f <= 1.0f ) {
		this->setPan( this->getPan() + 0.1f );
		qDebug() << "Head Control: Panning Left - " << this->getPan() << endl;
	}
}

void AiboHeadControl::panRight() {
	if( this->getPan() - 0.01f >= -1.0f ) {
		this->setPan( this->getPan() - 0.1f );
		qDebug() << "Head Control: Panning Right - " << this->getPan() << endl;
	}
}

void AiboHeadControl::tiltUp() {
	if( this->getTilt() + 0.01f <= 1.0f ) {
		this->setTilt( this->getTilt() + 0.1f );
		qDebug() << "Head Control: Titling Up - " << this->getTilt() << endl;
	}
}

void AiboHeadControl::tiltDown() {
	if( this->getTilt() - 0.01f >= -1.0f ) {
		this->setTilt( this->getTilt() - 0.1f );
		qDebug() << "Head Control: Titling Down - " << this->getTilt() << endl;
	}
}

void AiboHeadControl::centerHead() {
	this->centerPan();
	this->centerRoll();
	this->centerTilt();
}

void AiboHeadControl::centerTilt()
{
	this->setTilt( RESET );
}

void AiboHeadControl::centerPan()
{
	this->setPan( RESET );
}

void AiboHeadControl::centerRoll()
{
	this->setRoll( RESET );
}

bool AiboHeadControl::isCentered()
{
	return this->getPan() == 0.0f && this->getRoll() == 0.0f && this->getTilt() == 0.0f;
}

float AiboHeadControl::convertToWithinOne( float value )
{
	if( value >= 1.0f )
		value = 1.0f;
	else if( value <= -1.0f )
		value = -1.0f;
	return value;
}

bool isWithinOne( float value ) {
	return value <= 1.0f && value >= -1.0f;
}
